# coding=utf-8
import warnings
import matplotlib # type: ignore
matplotlib.use('TkAgg')
import numpy as np # type: ignore
import matplotlib.pyplot as plt # type: ignore
from mpl_toolkits.mplot3d import Axes3D
import rclpy
from rclpy.node import Node
from rclpy.timer import Timer
import socket
import struct
import rclpy.qos
from rcl_interfaces.srv import GetParameters
from geometry_msgs.msg import Twist, PoseStamped, Pose
from scipy.spatial.transform import Rotation as R # type: ignore
from std_msgs.msg import Float64MultiArray
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
from mavros_msgs.msg import PositionTarget
from laser_geometry import LaserProjection
from sensor_msgs.msg import NavSatFix,LaserScan,PointCloud2,Image
import sys, select, os
import tty, termios
import signal
import math

# 关闭 Matplotlib GUI 相关的警告
warnings.filterwarnings("ignore", category=UserWarning, message="Starting a Matplotlib GUI outside of the main thread")
EARTH_RADIUS = 6371008.7714150598
class consensus_APF_uav(Node):
    def __init__(self):
        # 初始化节点，指定节点名称
        super().__init__('consenses_APF_uav')
        self.folnum = int(sys.argv[1]) #无人机数量
        self.goal = [0, 0, 0]  # 目标点的坐标
        self.formation = '0' #队形
        # self.original_position = [113.9007328, 22.5181066, 1.0]
        self.original_position = [0.0, 0.0, 0.0]
        self.calculated_obstacles = []

        # 队形相对位置定义 (领导者与跟随者之间的相对位置)
        self.delta_x = np.array([-3, -3, -3, 0, 0, 0, 0, 0, 0])  # x方向
        self.delta_y = np.array([-6, -3, 0, 0, -6, -3, 0, 3, 0])  # y方向
        self.delta_z = np.array([-6, -3, 0, 0, -6, -3, 0, 3, 0])

        self.delta_x_1 = np.array([-6, -6, -6, -6, -3, -3, 0, -3, 0]) #([0, 0, 0, 1])#([-3, -3, -3, -3, 0, 0, 0, 0, 0])  # 1号队形
        self.delta_y_1 = np.array([-9, -3, 3, 9, -6, 0, 0, 6, 0])#([4, 0, -4, 1])#([-6, -3, 0, 3, -6, -3, 0, 3, 0])
        self.delta_z_1 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])#([0, 0, 0, 0])

        self.delta_x_2 = np.array([0, 5, 0, 1])#([-6, -6, -9, -9, -3, -3, 0, 0, 0])  # 2号队形
        self.delta_y_2 = np.array([2, 0, -2, 1])#([0, 3, 0, 3, 0, 3, 0, 3, 0])
        self.delta_z_2 = np.array([0, 0, 0, 0])

        self.delta_x_3 = np.array([-6, -6, -6, -6, -3, -3, 0, -3, 0])  # 3号队形
        self.delta_y_3 = np.array([-4.5, -1.5, 1.5, 4.5, -3, 0, 0, 3, 0])
        self.delta_z_3 = np.array([-4.5, -1.5, 1.5, 4.5, -3, 0, 0, 3, 0])


        self.init_f = np.zeros((self.folnum+1, 4))  # 初始化8个无人机和虚拟leader的坐标
        self.timestamps = [None] * self.folnum  # 初始化8个时间戳列表（假设8个无人机，0号无人机不需要）

        # 动态生成 Pose 对象列表，每个 Pose 对象存储无人机的当前位置
        # self.poses = [Pose() for _ in range(self.folnum)]  # 假设有folnum个无人机
        # self.poses = [PositionTarget() for _ in range(self.folnum)]
        self.poses = [Float64MultiArray() for _ in range(self.folnum)]
        self.uav_pose_pubs = {}

        group1 = MutuallyExclusiveCallbackGroup()
        group2 = MutuallyExclusiveCallbackGroup()

        qos_profile_1 = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, 
        history=rclpy.qos.HistoryPolicy.KEEP_LAST,              
        depth=10
        )
        qos_profile_2 = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, 
        history=rclpy.qos.HistoryPolicy.KEEP_LAST,              
        depth=1
        )
        # for i in range(self.folnum):
        #     index = i+1
        #     topic = "/px4{}/mavros/setpoint_raw/local".format(index)
        #     if i not in self.uav_pose_pubs:
        #         self.uav_pose_pubs[i] = self.create_publisher(PositionTarget, topic, 10)
        for i in range(self.folnum):
            index = i+1
            topic = "/drone{}/setpoint_raw/local".format(index)
            if i not in self.uav_pose_pubs:
                self.uav_pose_pubs[i] = self.create_publisher(Float64MultiArray, topic, 10)
        # # 监听激光雷达点云数据
        # self.vehicle_scan_subscriber = self.create_subscription(
        #         LaserScan,'/scan', self.vehicle_scan_callback, 10)   
        # #转换Laser数据到PointCloud2
        # self.laserProj = LaserProjection()
        # self.laser_to_pointcloud_publisher = self.create_publisher(
        #         PointCloud2, '/scan_pcl2', 10)

        self.subscription = self.create_subscription(
            PoseStamped,  # 消息类型
            '/consensus_APF/command',  # 话题名称
            self.consensus_APF_command_callback,  # 回调函数
            qos_profile_2,
            callback_group=group1
        )
        # # 接收障碍物点云参数
        # self.client = self.create_client(GetParameters,
        #                                  '/point_cloud_extractor/get_parameters')
        # request = GetParameters.Request()
        # request.names = ['calculated_obstacles']

        # self.client.wait_for_service()

        # future = self.client.call_async(request)
        # future.add_done_callback(self.callback_global_param)        
        self.client = self.create_client(GetParameters,
                                         '/goal_node/get_parameters')

        request = GetParameters.Request()
        request.names = ['calculated_obstacles']

        self.client.wait_for_service()

        future = self.client.call_async(request)
        future.add_done_callback(self.callback_global_param_from_goal_node)

        for i in range(self.folnum):  # 创建话题名称
            index = i+1
            # topic_name = "/px4{}/mavros/local_position/pose".format(index)
            topic_name = "/drone{}/velocity_and_position".format(index)
            self.get_logger().info(f"Listen to {topic_name}")

            self.create_subscription(
                Float64MultiArray,
                topic_name,
                lambda msg, id=i: self.pose_callback(msg, id),
                qos_profile_1,
                callback_group=group2
            )
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.server_address = ('192.168.1.240', 12345)
        # 打印启动日志
        self.get_logger().info("'consenses_APF_uav' node has been started!")


    def vehicle_scan_callback(self, vehicle_scan):
        cloud_out = self.laserProj.projectLaser(vehicle_scan)
        self.laser_to_pointcloud_publisher.publish(cloud_out)

    def callback_global_param(self, future):
        try:
            result = future.result()
        except Exception as e:
            self.get_logger().warn("service call failed %r" % (e,))
            # self.calculated_obstacles = []
        else:
            param = result.values[0]    
            print(param)       
            data = param.double_array_value.tolist()
            # print(data)
            # 确保列表的长度为 3 的倍数
            if len(data) % 3 != 0:
                data = data[:(len(data) // 3) * 3]
            # 将处理后的列表转换为二维数组，每行包含 3 个元素
            self.calculated_obstacles = np.array(data).reshape(-1, 3)
            # print(self.calculated_obstacles)
            self.get_logger().info("Got obstacles param: %s" % (self.calculated_obstacles,))

    def callback_global_param_from_goal_node(self, future):

        print("我在这里？？？？？")
        try:
            result = future.result()
        except Exception as e:
            self.get_logger().warn("service call failed %r" % (e,))
            # self.calculated_obstacles = []
        else:
            param = result.values[0]    
            # print(param)       
            data = param.double_array_value.tolist()
            # print(data)
            # 确保列表的长度为 3 的倍数
            if len(data) % 3 != 0:
                data = data[:(len(data) // 3) * 3]

            print(f"original_position:{self.original_position}")

            for index in range(0, len(data), 3):
                data[index] = (data[index]-self.original_position[0])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
                data[index+1] = (data[index+1]-self.original_position[1])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
                data[index+2] = data[index+2] - self.original_position[2]
            # 将处理后的列表转换为二维数组，每行包含 3 个元素
            self.calculated_obstacles = np.array(data).reshape(-1, 3)
            print(self.calculated_obstacles)
            self.get_logger().info("Got obstacles param: %s" % (self.calculated_obstacles,))            
    
    def compute_area(self, x, y, z, range=10):  
        '''计算动画显示的区域'''
        if x >= 0:
            num_x = np.floor(x / range)
            x1 = num_x * range
            x2 = (num_x + 1) * range
        else:
            num_x = np.floor(x / -range)
            x1 = -range * (num_x + 1)
            x2 = -range * num_x
        if y >= 0:
            num_y = np.floor(y / range)
            y1 = num_y * range
            y2 = (num_y + 1) * range
        else:
            num_y = np.floor(y / -range)
            y1 = -range * (num_y + 1)
            y2 = -range * num_y
        if z >= 0:
            num_z = np.floor(z / range)
            z1 = num_z * range
            z2 = (num_z + 1) * range
        else:
            num_z = np.floor(z / -range)
            z1 = -range * (num_z + 1)
            z2 = -range * num_z
        # area = [x1 - 8, x2 + 8, y1 - 8, y2 + 8, z1 -8, z2 + 8]
        area = [x1 - 200, x2 + 200, y1 - 200, y2 + 200]
        return area
    
    def compute_repulsion(self, robot_pose, obs_pose, detect_R):
        '''计算斥力函数'''
        repulsion = np.array([0.0, 0.0, 0.0]) #不改变z轴的值
        for obs in obs_pose:
            distance = np.sqrt((robot_pose[0] - obs[0]) ** 2 + (
                        robot_pose[1] - obs[1]) ** 2)  # 计算当前无人机和障碍物的平面距离（点云里怎么获取这个obs_pose，现在还都是自己设定的）

            # 检查距离是否为零，避免除以零
            if distance == 0:
                continue  # 跳过当前障碍物

            if distance <= detect_R:
                temp = (1 / distance - 1 / detect_R) / (distance ** 3)  # 计算斥力大小，用经验推导的斥力函数

                # 检查临时变量是否为无效值，避免产生无效值
                if not np.isnan(temp) and not np.isinf(temp):
                    repulsion[0] += temp * (robot_pose[0] - obs[0])
                    repulsion[1] += temp * (robot_pose[1] - obs[1])
                    # repulsion[2] += temp * (robot_pose[2] - obs[2])
        return repulsion
    
    def confine(self, current, next, Kinematic, dt):  
        ''' 基于运动学给无人机速度做约束，给定的最大速度和加速度限制'''
        # Kinematic = np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5])
        # Limitation on speed x
        delta_x = next[0] - current[0]  # 计算无人机在 x 方向上的速度变化量
        if delta_x >= 0:
            next[0] = min(current[0] + delta_x, current[0] + Kinematic[0] * dt)
        else:
            next[0] = max(current[0] + delta_x, current[0] - Kinematic[0] * dt)
        if next[0] >= 0:
            next[0] = min(next[0], Kinematic[0])
        else:
            next[0] = max(next[0], -Kinematic[0])
        # Limitation on speed y
        delta_y = next[1] - current[1]
        if delta_y >= 0:
            next[1] = min(current[1] + delta_y, current[1] + Kinematic[1] * dt)
        else:
            next[1] = max(current[1] + delta_y, current[1] - Kinematic[1] * dt)
        if next[1] >= 0:
            next[1] = min(next[1], Kinematic[1])
        else:
            next[1] = max(next[1], -Kinematic[1])
        # Limitation on speed z
        delta_z = next[2] - current[2]
        if delta_z >= 0:
            next[2] = min(current[2] + delta_z, current[2] + Kinematic[2] * dt)
        else:
            next[2] = max(current[2] + delta_z, current[2] - Kinematic[2] * dt)
        if next[2] >= 0:
            next[2] = min(next[2], Kinematic[2])
        else:
            next[2] = max(next[2], -Kinematic[2])
        return next
    
    def draw_circle(self, x, y, r, state):  
        ''' 画圆形的函数'''
        color = ''
        line = 1
        if state == 1:
            color = 'b'
        elif state == 2:
            color = 'r'
            line = 2
        elif state == 3:
            color = 'g'  # 添加额外的颜色选择
            line = 1
        elif state == 4:
            color = 'c'  # 添加额外的颜色选择
            line = 1
        elif state == 5:
            color = 'm'  # 添加额外的颜色选择
            line = 1
        elif state == 6:
            color = 'y'  # 添加额外的颜色选择
            line = 1
        elif state == 7:
            color = 'k'  # 添加额外的颜色选择
            line = 1
        t = np.arange(0, 2 * np.pi, 0.01)
        X = x + r * np.cos(t)
        Y = y + r * np.sin(t)
        plt.plot(X, Y, color=color, linewidth=line)

    def yaw_to_orientation(self,yaw):  # yaw角转换为orientation
        # Convert yaw angle in radians to quaternion
        yaw -= 1.57
        quaternion = R.from_euler('z', yaw).as_quat()
        return quaternion

    def consensus_APF_command_callback(self, msg):
        self.formation = msg.header.frame_id
        self.goal[0] = msg.pose.position.x
        self.goal[1] = msg.pose.position.y
        self.goal[2] = msg.pose.position.z
        # self.get_logger().info(f'Received formation_id: {msg.data}')
        self.get_logger().info(f'Received formation_id: {msg.header.frame_id},'f'Received PoseStamped: Position({msg.pose.position.x}, {msg.pose.position.y}, {msg.pose.position.z})')
    
    def consensus_APF(self, pose_x, pose_y, pose_z, pose_th, goal, ob_temp, A, delta_x, delta_y, delta_z, countmax, fol_num, N, detect_R, Kinematic, dt, k): 
        '''一致性编队+人工势场法避障'''
        count = 0
        KN = 1  # KN的值需要根据实际情况设定,吸引力增益,增大KN会使无人机更快地向目标点靠近
        beta = 5  # beta的值需要根据实际情况设定,排斥力增益,增大beta能增强无人机避开障碍的能力
        d_max = 4  # d_max的值需要根据实际情况设定,无人机对目标点或其他无人机的最大响应距离
        gama = 0.5  # gama的值需要根据实际情况设定,一致性增益
        K0 = 1 - gama  # K0的值需要根据实际情况设定,速度缩放因子
        V_x = np.zeros((N, countmax + 2))  # 无人机集群x方向的速度
        V_y = np.zeros((N, countmax + 2))  # 无人机集群y方向的速度
        V_z = np.zeros((N, countmax + 2))  # 无人机集群y方向的速度
        plt.ion()  # 打开交互模式
        fig = plt.figure()  # 创建一个新的图形窗口
        # has_obs = 0
        # V_max = 0.5
        # 设定一个阈值epsilon，表示目标点足够接近时认为到达
        epsilon = 1.0
        while count < countmax:
            if self.formation == '0':  # 保持当前队形
                delta_x = self.delta_x
                delta_y = self.delta_y
                delta_z = self.delta_z
            elif self.formation == '1':  # 1号队形
                delta_x = self.delta_x_1
                delta_y = self.delta_y_1
                delta_z = self.delta_z_1
            elif self.formation == '2':  # 2号队形
                delta_x = self.delta_x_2
                delta_y = self.delta_y_2
                delta_z = self.delta_z_2
            elif self.formation == '3':  # 3号队形
                delta_x = self.delta_x_3
                delta_y = self.delta_y_3
                delta_z = self.delta_z_3
            end_point_x = self.goal[0] #node.get_parameter('/end_point_x').get_parameter_value().integer_value
            end_point_y = self.goal[1] #node.get_parameter('/end_point_y').get_parameter_value().integer_value
            end_point_z = self.goal[2]
            goal = (end_point_x, end_point_y, end_point_z)  # 修改目标点位置
            count += 1
            k += 1
            distance = np.sqrt((goal[0] - pose_x[N - 1, k]) ** 2 + (goal[1] - pose_y[N - 1, k]) ** 2 + (goal[2] - pose_z[N - 1, k]) ** 2)
            distance_xy = np.sqrt((goal[0] - pose_x[N - 1, k]) ** 2 + (goal[1] - pose_y[N - 1, k]) ** 2)
            delta = np.array([goal[0] - pose_x[N - 1, k], goal[1] - pose_y[N - 1, k], goal[2] - pose_z[N - 1, k]])
            
            th = np.arctan2(goal[1] - pose_y[N - 1, k], goal[0] - pose_x[N - 1, k])
            # gam = np.arctan2(goal[2] - pose_z[N - 1, k], distance_xy)
            # if distance > d_max:
                # distance = d_max
            # 根据当前领导者和目标距离和角度计算领导者速度
            # V_x[N - 1, k + 1] = KN * distance * np.cos(gam) * np.cos(th)
            # V_y[N - 1, k + 1] = KN * distance * np.cos(gam) * np.sin(th)
            # V_z[N - 1, k + 1] = KN * distance * np.sin(gam)
            # ob_pose = ob_temp #障碍物位置
            # repulsion = compute_repulsion([pose_x[N-1, k], pose_y[N-1, k]], ob_pose, detect_R)
            # V_x[N-1, k+1] += beta * repulsion[0] #把斥力加权到速度上
            # V_y[N-1, k+1] += beta * repulsion[1]
            # 根据雷达获取的障碍物位置更新ob_pose，用get_param
            # if self.has_parameter("/obs_position") and has_obs == 0:
            #     ob_temp = self.get_parameter("/obs_position").get_parameter_value().string_array_value
            #     ob_temp = np.array(ob_temp)
            #     print(ob_temp)
            # has_obs = 1
            if self.calculated_obstacles is not None:  # 判断self.calculated_obstacles是否非空
                ob_temp = self.calculated_obstacles
            ob_pose = ob_temp
            repulsion = self.compute_repulsion([pose_x[N - 1, k], pose_y[N - 1, k], pose_z[N - 1, k]], ob_pose, detect_R) #计算平面内的障碍物的斥力
            # V_x[N-1, k+1] += beta * repulsion[0] #加权到速度上
            # V_y[N-1, k+1] += beta * repulsion[1]
            
            # 判断是否已经到达目标点
            if distance < epsilon:
                print(f"我是{distance}  epsilon:{epsilon}")
                V_x[N - 1, k + 1] = 0.0  # 更新速度
                V_y[N - 1, k + 1] = 0.0
                V_z[N - 1, k + 1] = 0.0
            else:
                # unitVec = delta / distance  # 继续向目标点移动
                # 将障碍物斥力加到领导者速度上
                previous_velocity = np.array([V_x[N - 1, k], V_y[N - 1, k], V_z[N - 1, k]])  # 计算之前的速度
                numerator = K0 * previous_velocity + gama * delta + beta * repulsion  # 计算 allF
                denominator = np.linalg.norm(numerator)
                allF = numerator / denominator
                V_x[N - 1, k + 1] = Kinematic[0] * allF[0]  # 更新速度
                V_y[N - 1, k + 1] = Kinematic[1] * allF[1]
                V_z[N - 1, k + 1] = Kinematic[2] * allF[2]

            if (distance > 1.0 and abs(V_x[N-1, k+1]) <= 0.1 and abs(V_y[N-1, k+1]) <= 0.1 and abs(V_z[N-1, k+1]) <= 0.1):
                V_x[N-1, k+1] += -1 + 2 * np.random.rand() #如果领导者速度很小，且还没到目标，给个随机速度
                V_y[N-1, k+1] += -1 + 2 * np.random.rand()
                V_z[N-1, k+1] += -1 + 2 * np.random.rand()

            for i in range(0, fol_num):#跟随者位置计算
                sum_delta_x = 0.0
                sum_delta_y = 0.0
                sum_delta_z = 0.0
                for j in range(0, N):
                    if A[i, j] == 1:
                        # w_ij = 2 - np.exp(-((pose_x[j, k-1] - pose_x[i, k] - (delta_x[j] - delta_x[i]))**2 + (pose_y[j, k-1] - pose_y[i, k] - (delta_y[j] - delta_y[i]))**2))
                        # sum_delta_x += A[i, j] * w_ij * ((pose_x[j, k-1] - pose_x[i, k]) - (delta_x[j] - delta_x[i]))
                        # sum_delta_y += A[i, j] * w_ij * ((pose_y[j, k-1] - pose_y[i, k]) - (delta_y[j] - delta_y[i]))
                        w_ij = 2 - np.exp(-((pose_x[j, k] - pose_x[i, k] - (delta_x[j] - delta_x[i]))**2 + (pose_y[j, k] - pose_y[i, k] - (delta_y[j] - delta_y[i]))**2 + (pose_z[j, k] - pose_z[i, k] - (delta_z[j] - delta_z[i]))**2))
                        sum_delta_x += A[i, j] * w_ij * ((pose_x[j, k] - pose_x[i, k]) - (delta_x[j] - delta_x[i]))
                        sum_delta_y += A[i, j] * w_ij * ((pose_y[j, k] - pose_y[i, k]) - (delta_y[j] - delta_y[i]))
                        sum_delta_z += A[i, j] * w_ij * ((pose_z[j, k] - pose_z[i, k]) - (delta_z[j] - delta_z[i]))
                distance = np.sqrt(sum_delta_x**2 + sum_delta_y**2 + sum_delta_z**2)
                # 判断是否已经到达目标点
                if distance < epsilon:
                    V_x[i, k + 1] = 0.0  # 更新速度
                    V_y[i, k + 1] = 0.0
                    V_z[i, k + 1] = 0.0
                else:
                    # distance_xy = np.sqrt(sum_delta_x**2 + sum_delta_y**2)
                    # th = np.arctan2(sum_delta_y, sum_delta_x)
                    # gam = np.arctan2(sum_delta_z, distance_xy)
                    reference_velocity = np.array([V_x[N - 1, k], V_y[N - 1, k], V_z[N - 1, k]])  # 计算虚拟leader的速度
                    delta = np.array([sum_delta_x, sum_delta_y, sum_delta_z])
                    
                    # V_x[i, k + 1] = KN * V_x[N-1, k] + gama * distance * np.cos(gam) * np.cos(th) #计算跟随者的下一时刻速度
                    # V_y[i, k + 1] = KN * V_y[N-1, k] + gama * distance * np.cos(gam) * np.sin(th)
                    # V_z[i, k + 1] = KN * V_z[N-1, k] + gama * distance * np.sin(gam)
                    # out = self.confine([V_x[i, k], V_y[i, k]], [V_x[i, k+1], V_y[i, k+1]], Kinematic, 0.1)
                    # V_x[i, k+1] = out[0]
                    # V_y[i, k+1] = out[1]
                    kk = 0 
                    obs_pose = np.zeros((N-1, 3))
                    for j in range(0, N-1): #计算和其他无人机之间的斥力
                        if j != i:
                            kk += 1
                            obs_pose[kk-1, 0] = pose_x[j, k]
                            obs_pose[kk-1, 1] = pose_y[j, k]
                            obs_pose[kk-1, 2] = pose_z[j, k]
                    ob_pose = np.vstack((obs_pose, ob_temp))
                    repulsion = self.compute_repulsion([pose_x[i, k], pose_y[i, k], pose_z[i, k]], ob_pose, detect_R)
                    # V_x[i, k+1] += beta * repulsion[0] #加权到速度上
                    # V_y[i, k+1] += beta * repulsion[1]
                    numerator = KN * reference_velocity + gama * delta + beta * repulsion
                    denominator = np.linalg.norm(numerator)
                    allF = numerator / denominator
                    V_x[i, k + 1] = Kinematic[0] * allF[0]  # 更新速度
                    V_y[i, k + 1] = Kinematic[1] * allF[1]
                    V_z[i, k + 1] = Kinematic[2] * allF[2]
                    if (distance > 1 and abs(V_x[i, k+1]) <= 0.1 and abs(V_y[i, k+1]) <= 0.1 and abs(V_z[i, k+1]) <= 0.1):
                        V_x[i, k+1] += -1 + 2 * np.random.rand() #如果速度很小，且还没到目标，给个随机速度
                        V_y[i, k+1] += -1 + 2 * np.random.rand()
                        V_z[i, k+1] += -1 + 2 * np.random.rand()

                print(f"V_x: {V_x[i, k+1]}  V_y: {V_y[i, k+1]}  V_z: {V_z[i, k+1]}")


            pose_xtemp=np.zeros((N,1))
            pose_ytemp=np.zeros((N,1))
            pose_ztemp=np.zeros((N,1))
            pose_thtemp=np.zeros((N,1))
            for i in range(0, N):
                # out = self.confine([V_x[i, k], V_y[i, k]], [V_x[i, k+1], V_y[i, k+1]], Kinematic, 0.1)
                # V_x[i, k+1] = out[0]
                # V_y[i, k+1] = out[1]
                pose_xtemp[i,0]=pose_x[i, k] + dt * V_x[i, k+1] #根据当前位置和速度计算下一时刻的位置
                pose_ytemp[i,0]=pose_y[i, k] + dt * V_y[i, k+1] #y and th error
                pose_ztemp[i,0]=pose_z[i, k] + dt * V_z[i, k+1]
                pose_thtemp[i,0]=np.arctan2(V_y[i, k+1], V_x[i, k+1])#计算yaw角
            pose_x=np.column_stack((pose_x,pose_xtemp))
            pose_y=np.column_stack((pose_y,pose_ytemp))
            pose_z=np.column_stack((pose_z,pose_ztemp))
            pose_th=np.column_stack((pose_th,pose_thtemp))

            # 给无人机发送位置和朝向角
            for uav_i in range(self.folnum):
                # self.poses[uav_i].type_mask = 0b010111111000
                # self.poses[uav_i].coordinate_frame = 1
                # self.poses[uav_i].position.x = pose_y[uav_i, k]
                # self.poses[uav_i].position.y = -pose_x[uav_i, k]
                # self.poses[uav_i].position.z = 2.0
                # self.poses[uav_i].yaw_rate = 0.0
                # header = 8.8
                longitude = self.original_position[0] + (pose_x[uav_i, k + 1] / (EARTH_RADIUS + self.original_position[2])) * 180 / math.pi
                latitude = self.original_position[1] + (pose_y[uav_i, k + 1] / (EARTH_RADIUS + self.original_position[2])) * 180 / math.pi
                altitude = self.original_position[2] + pose_z[uav_i, k + 1]
                velocity_east = V_x[uav_i, k+1]
                velocity_north = V_y[uav_i, k+1]
                velocity_up = V_z[uav_i, k+1]
                self.poses[uav_i].data = [longitude, latitude, altitude, velocity_east, velocity_north, velocity_up]
                # print(f"original_position: x:{self.original_position[0]}  y:{self.original_position[1]},--pose: x:{pose_x[uav_i, k]}  y:{pose_y[uav_i, k]}")
                print(f"lon: {longitude}--lat: {latitude}--alt: {altitude}")
                # orientation = self.yaw_to_orientation(pose_th[uav_i, k])
                # self.poses[uav_i].orientation.x = orientation[0]
                # self.poses[uav_i].orientation.x = orientation[1]
                # self.poses[uav_i].orientation.x = orientation[2]
                # self.poses[uav_i].orientation.x = orientation[3]
                #UDP
                status_data = struct.pack(
                'iiiiii32siiii32sddddddddd????????',
                1, 1, 1, 1, 1, uav_i, f"UAV{uav_i+1}".encode('ascii'), 1, 1, 1, 1, "uav".encode('ascii'),
                 #mode_type, t, colonyID, subcolconyID, formation_type, uavIndex, uavName.encode('ascii'), taskID, taskType, model, node, camp.encode('ascii'),
                longitude, latitude, altitude, velocity_east, velocity_north, velocity_up, 0, 0, 0,
                True, True, True, True,
                True, True, True, True
                )
                print(f"len: {len(status_data)}")
                self.sock.sendto(status_data, self.server_address)


            # 发布对应的 poses
            for i in range(self.folnum):
                self.uav_pose_pubs[i].publish(self.poses[i])

            # ====动画显示====
            # print(k)
            area = self.compute_area(pose_x[N - 2, k + 1], pose_y[N - 2, k + 1], pose_z[N - 2, k + 1], N + 1) #暂时修改为二维
            ArrowLength = 0.5  # 箭头长度
            plt.ion()  # 打开交互模式
            plt.clf()
            for j in range(N - 1):
                plt.quiver(pose_x[j, k + 1], pose_y[j, k + 1], ArrowLength * np.cos(pose_th[j, k + 1]),
                        ArrowLength * np.sin(pose_th[j, k + 1]), scale=1, scale_units='xy', color='k',
                        width=0.01)  # 绘制机器人的方向
                if j == N:
                    state = 2  # 领导者标记
                else:
                    state = 1  # 跟随者标记
                self.draw_circle(pose_x[j, k + 1], pose_y[j, k + 1], 0.25, state)  # 绘制机器人
                # for i in range(N):
                # if A[i, j] == 1:
                # draw_arrow([pose_x[j, k+1], pose_y[j, k+1]], [pose_x[i, k+1], pose_y[i, k+1]], 0.2)  # 绘制连接箭头，显示通信或控制关系

                if len(ob_temp) != 0:
                    plt.plot(ob_temp[:, 0], ob_temp[:, 1], 'Xk', linewidth=2)  # 绘制障碍物位置

            plt.axis(area)  # 设置显示区域
            plt.grid(True)  # 显示网格
            plt.draw()  # 实时更新图形
            plt.pause(0.01)  # 添加短暂的暂停以显示图形
            # plt.show()
            # 判断是否到达目标
            difference_now = np.sqrt((goal[0] - pose_x[N - 1, k + 1]) ** 2 + (goal[1] - pose_y[N - 1, k + 1]) ** 2 + (goal[2] - pose_z[N - 1, k + 1]) ** 2)
            # now = [pose_x[N - 1, k + 1], pose_y[N - 1, k + 1]]  # 获取领导者当前位置
            # difference_now = [now[i] - goal[i] for i in range(len(now))]
            if difference_now < epsilon:
                print('Arrive Goal!!')  # 如果领导者到达目标点附近，则显示到达信息并结束循环
            # arrive_time+=1
            # if arrive_time%3==0 and path_times<path_len-1:
            # path_times+=1
            plt.ioff()  # 关闭交互模式
            # # ====三维动画显示====
            # # print(k)
            # area = self.compute_area(pose_x[N - 2, k + 1], pose_y[N - 2, k + 1], pose_z[N - 2, k + 1], N + 1) #暂时修改为二维
            # ArrowLength = 0.5  # 箭头长度
            
            # ax = fig.add_subplot(111, projection='3d')  # 创建3D子图
            

            # # 绘制每个机器人
            # for j in range(N - 1):
            #     ax.cla()  # 清空之前的图形
            #     # plt.quiver(pose_x[j, k + 1], pose_y[j, k + 1], ArrowLength * np.cos(pose_th[j, k + 1]),
            #     #         ArrowLength * np.sin(pose_th[j, k + 1]), scale=1, scale_units='xy', color='k',
            #     #         width=0.01)  # 绘制机器人的方向
            #     # ax.quiver(pose_x[j, k + 1], pose_y[j, k + 1], pose_z[j, k + 1], 
            #     #   ArrowLength * np.cos(pose_th[j, k + 1]), 
            #     #   ArrowLength * np.sin(0), 
            #     #   0, scale=1, color='k', linewidth=1) 
            #     ax.quiver(pose_x[j, k + 1], pose_y[j, k + 1], pose_z[j, k + 1], 
            #                 V_x[i, k+1], V_y[i, k+1], V_z[i, k+1], length=0.1, normalize=True)
            #                 # 绘制机器人运动方向
            #     if j == N:
            #         state = 2  # 领导者标记
            #     else:
            #         state = 1  # 跟随者标记
            #     # self.draw_circle(pose_x[j, k + 1], pose_y[j, k + 1], 0.25, state)  # 绘制机器人
            #     self.draw_circle(pose_x[j, k + 1], pose_y[j, k + 1], 0.25, state)  # 绘制机器人位置
            #     # for i in range(N):
            #     # if A[i, j] == 1:
            #     # draw_arrow([pose_x[j, k+1], pose_y[j, k+1]], [pose_x[i, k+1], pose_y[i, k+1]], 0.2)  # 绘制连接箭头，显示通信或控制关系

            #     if ob_temp.size != 0:
            #         # plt.plot(ob_temp[:, 0], ob_temp[:, 1], 'Xk', linewidth=2)  # 绘制障碍物位置
            #         ax.scatter(ob_temp[:, 0], ob_temp[:, 1], ob_temp[:, 2], color='k', marker='X', s=50, label="Obstacles")

            # # plt.axis(area)  # 设置显示区域
            # # 设置显示区域 (在3D中需要设置x, y, z轴的范围)
            # ax.set_xlim([area[0], area[1]])
            # ax.set_ylim([area[2], area[3]])
            # ax.set_zlim([area[4], area[5]])
            # ax.grid(True)  # 显示网格
            # plt.draw()  # 实时更新图形
            # plt.pause(0.001)  # 添加短暂的暂停以显示图形
            # # plt.show()
            # # 判断是否到达目标
            # now = [pose_x[N - 1, k + 1], pose_y[N - 1, k + 1], pose_z[N - 1, k + 1]]  # 获取领导者当前位置
            # difference_now = [now[i] - goal[i] for i in range(len(now))]
            # if np.linalg.norm(difference_now) < 0.5:
            #     print('Arrive Goal!!')  # 如果领导者到达目标点附近，则显示到达信息并结束循环
            # # arrive_time+=1
            # # if arrive_time%3==0 and path_times<path_len-1:
            # # path_times+=1
            # plt.ioff()  # 关闭交互模式
            # # plt.show()  # 最后显示图形
        print('Finished!!!')
        # 定义颜色和线类型
        color = ['m', 'g', 'b', 'k', 'r', 'c', 'y', 'purple']
        # type = [2, 1, 0.5, 0.5, 2, 2]  # 这里的线类型在ROS中可能不适用，需要根据具体情况修改

        # 绘制机器人的移动路径
        # plt.figure()  # 新建一个图形窗口
        # for i in range(N - 1):
        #     plt.plot(pose_x[i, :], pose_y[i, :], color[i], linewidth=2)  # 绘制每个机器人的轨迹
        # for i in range(N - 2):
        #     plt.plot(pose_x[i, 0], pose_y[i, 0], 'bp', color=color[i], linewidth=1)  # 标记每个跟随者的起始位置
        # # plt.plot(pose_x[N - 1, 0], pose_y[N - 1, 0], '*', color=color[N - 1], linewidth=1)  # 标记领导者的起始位置
        # for i in range(N - 2):
        #     plt.plot(pose_x[i, k], pose_y[i, k], 'm^', color=color[i], linewidth=2)  # 标记每个跟随者的终止位置
        # # plt.plot(pose_x[N - 1, k], pose_y[N - 1, k], 'o', color=color[N - 1], linewidth=2)  # 标记领导者的终止位置

        # # 绘制障碍物的位置
        # if ob_temp.size != (0, 0):
        #     plt.plot(ob_temp[:, 0], ob_temp[:, 1], 'Xk', linewidth=2)

        # plt.grid(True)  # 显示网格
        # plt.xlabel('x(m)')  # x轴标签，添加单位
        # plt.ylabel('y(m)')  # y轴标签，添加单位
        # plt.title('Formation Consensus')  # 给图形添加标题
        # plt.legend(
        #     ['UAV 1', 'UAV 2', 'UAV 3', 'UAV 4', 'UAV 5', 'UAV 6', 'UAV 7', 'UAV 8', 'Virtual Leader'])  # 添加图例，区分各个机器人
        # plt.show()
        # 创建一个新的图形窗口
        fig1 = plt.figure()

        # 创建一个三维坐标轴对象
        ax1 = fig1.add_subplot(111, projection='3d')

        # 假设 pose_x, pose_y, pose_z 是你的数据，表示机器人在三维空间中的路径
        for i in range(N - 1):
            ax1.plot(pose_x[i, :], pose_y[i, :], pose_z[i, :], color[i], linewidth=2)  # 绘制每个机器人的三维轨迹

        # 标记每个机器人的起始位置（假设它们在 (pose_x[i, 0], pose_y[i, 0], pose_z[i, 0])）
        for i in range(N - 2):
            ax1.scatter(pose_x[i, 0], pose_y[i, 0], pose_z[i, 0], color=color[i], marker='p', s=100)  # 起始位置

        # 标记每个机器人的终止位置（假设它们在 (pose_x[i, k], pose_y[i, k], pose_z[i, k])）
        for i in range(N - 2):
            ax1.scatter(pose_x[i, k], pose_y[i, k], pose_z[i, k], color=color[i], marker='^', s=100)  # 终止位置

        # 如果有障碍物的位置，绘制障碍物（假设 ob_temp 存储障碍物的 3D 坐标）
        if ob_temp.size != (0, 0, 0):
            ax1.scatter(ob_temp[:, 0], ob_temp[:, 1], ob_temp[:, 2], color='k', marker='X', s=100)  # 障碍物位置

        # 设置坐标轴标签
        ax1.set_xlabel('X (m)')
        ax1.set_ylabel('Y (m)')
        ax1.set_zlabel('Z (m)')

        # 添加标题
        ax1.set_title('Formation Consensus in 3D')

        # 显示网格
        ax1.grid(True)

        # 图例
        ax1.legend(
            ['UAV 1', 'UAV 2', 'UAV 3', 'UAV 4', 'UAV 5', 'UAV 6', 'UAV 7', 'UAV 8', 'Virtual Leader']
        )

        # 显示图形
        plt.show()
    
    def pose_callback(self, msg, drone_id):  
        self.get_logger().info(f"Drone_{drone_id+1} is getting position!")
        #global init_f, timestamps
        # 从消息中读取位置数据
        # x = msg.pose.position.x
        # y = msg.pose.position.y
        # z = msg.pose.position.z
        # longitude = msg.data[1]  # 假设单位为度*1E7
        # latitude = msg.data[2]
        # altitude = msg.data[3] 
        longitude = msg.data[0]  # 假设单位为度*1E7
        latitude = msg.data[1]
        altitude = msg.data[2] 
        print(f"get  lon: {longitude}--lat: {latitude}--alt: {altitude}")
        # velocity_east = int(msg.data[4] * 100)  # 假设单位为cm/s
        # velocity_north = int(msg.data[5] * 100)
        # velocity_up = int(msg.data[6] * 100)
        if drone_id == 0:
            self.original_position[0] = longitude
            self.original_position[1] = latitude
            self.original_position[2] = altitude
        else:
            if self.original_position[0] == 0:
                return
        current_time = msg.data[0] # 获取消息的时间戳
        # 将位置数据存入位置矩阵的对应行
        # for i in range(self.folnum):
            # x = (longitude-self.original_position[0])/180*math.pi*(EARTH_RADIUS + self.original_position[2])+i*5.0
            # y = (latitude-self.original_position[1])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
            # z = altitude - self.original_position[2]
            # # self.init_f[drone_id] = [x, y, z, 0]
            # self.init_f[i] = [x, y, z, 0]
            # self.init_f[self.folnum] = [0, 0, 0, 0]
            # self.timestamps[i] = current_time  # 记录时间戳
            # self.delta_x[self.folnum] = self.init_f[self.folnum][0]
            # self.delta_y[self.folnum] = self.init_f[self.folnum][1]
            # self.delta_z[self.folnum] = self.init_f[self.folnum][2]

            # self.delta_x[i] = x
            # self.delta_y[i] = y
            # self.delta_z[i] = z
            # print(f"get  delta_x: {self.delta_x[i]}--delta_y: {self.delta_y[i]}--delta_z: {self.delta_z[i]}--drone id: {i+1}")
        x = (longitude-self.original_position[0])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
        y = (latitude-self.original_position[1])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
        z = altitude - self.original_position[2]
        # self.init_f[drone_id] = [x, y, z, 0]
        self.init_f[drone_id] = [x, y, z, 0]
        self.init_f[self.folnum] = [0, 0, 0, 0]
        self.timestamps[drone_id] = current_time  # 记录时间戳
        self.delta_x[self.folnum] = self.init_f[self.folnum][0]
        self.delta_y[self.folnum] = self.init_f[self.folnum][1]
        self.delta_z[self.folnum] = self.init_f[self.folnum][2]

        self.delta_x[drone_id] = x
        self.delta_y[drone_id] = y
        self.delta_z[drone_id] = z
        print(f"get  delta_x: {self.delta_x[drone_id]}--delta_y: {self.delta_y[drone_id]}--delta_z: {self.delta_z[drone_id]}--drone id: {drone_id +1}")
    
        if all(timestamp is not None for timestamp in self.timestamps):
            self.perform_consistency_algorithm()
    
    def perform_consistency_algorithm(self):
        # self.get_logger().info("perform_consistency_algorithm")    
        N = self.folnum+1  # 机器人个数（跟随者和虚拟领导者）
        countmax = 5000  # 最大循环次数
        dt = 0.1  # 控制和通信周期

        k = 0  # 迭代次数
        # 机器人的动力学参数：速度xyz：m/s，角速度：rad/s，加速度：m/s^2 和 rad/s^2
        Kinematic = np.array([3, 3, 3, 0.5, 0.5, 0.5, 0.5])

        # 邻接矩阵
        A = np.array([[1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1]])
        # A = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [0, 0, 0, 0, 0, 0, 0, 0, 1],
        #             [1, 1, 1, 1, 1, 1, 1, 1, 0]])

        pose_x = self.init_f[:, 0]  # 从init_f提取所有机器人的x坐标
        pose_y = self.init_f[:, 1]  # 从init_f提取所有机器人的y坐标
        pose_z = self.init_f[:, 2]  # 从init_f提取所有机器人的z坐标
        pose_th = self.init_f[:, 3]  # 从init_f提取所有机器人的朝向角度

        pose_x = np.column_stack((self.init_f[:, 0], pose_x))  # 创建一个二维数组，第一列是初始x坐标，第二列是预留空间
        pose_y = np.column_stack((self.init_f[:, 1], pose_y))  # 创建一个二维数组，第一列是初始y坐标，第二列是预留空间
        pose_z = np.column_stack((self.init_f[:, 2], pose_z))  # 创建一个二维数组，第一列是初始z坐标，第二列是预留空间
        pose_th = np.column_stack((self.init_f[:, 3], pose_th))  # 创建一个二维数组，第一列是初始朝向角度，第二列是预留空间

        # 障碍物的位置
        # ob_temp = np.array([])
        ob_temp = np.array([[-10.0,-10.0,0.0],
                            [10.0, 20.0, 0.0],
                            [30.0, 40.0, 0.0],
        ])
        

        # 共识算法中的相对位置 (在这里设置队形)
        # V_x = np.zeros((N, 1))  # 初始化速度矩阵x分量
        # V_y = np.zeros((N, 1))  # 初始化速度矩阵y分量
        # d_max = 2  # 最大距离
        detect_R = 2  # 障碍物检测范围
        self.consensus_APF(pose_x, pose_y, pose_z, pose_th, self.goal, ob_temp, A, self.delta_x, self.delta_y, self.delta_z, countmax, self.folnum, N, detect_R,
                    Kinematic, dt, k)

def main(args=None):
    # 初始化 ROS 2 客户端库
    rclpy.init(args=args)
    
    # 创建并运行 consensus_APF_uav 实例
    node = consensus_APF_uav()

    # 使用 MultiThreadedExecutor 来支持并行回调
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    
    # 让节点保持运行并处理回调函数
    executor.spin()
    #rclpy.spin(node)

    # 销毁节点并关闭 ROS 2 客户端
    executor.shutdown()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
